/*
 * motormotion.cxx
 *
 *  Created on: Sep 7, 2013
 *      Author: jacob
 */
#include "InertialData.h"




int sockfd;
struct sockaddr_in self;
char buffer[MAXBUF];
int ret = 0;
int clientfd;
struct sockaddr_in client_addr;
socklen_t addrlen=sizeof(client_addr);

InertialData::InertialData() {
	//printf("Hello World!\n");

	if (UDP)
	{
		/*---Create datagram socket---*/
		if ( (sockfd = socket(AF_INET, SOCK_DGRAM, 0)) < 0 )
		{
			perror("Socket");
			exit(errno);
		}
		else;
	}
	else
		/*---Create streaming socket---*/
		if ( (sockfd = socket(AF_INET, SOCK_STREAM, 0)) < 0 )
		{
			perror("Socket");
			exit(errno);
		}
		else;

	/*---Initialize address/port structure---*/
	//bzero(&self, sizeof(self));
	memset(&self,0,sizeof(self));
	self.sin_family = AF_INET;
	self.sin_port = htons(MY_PORT);
	self.sin_addr.s_addr = htonl(INADDR_ANY);

	/*---Assign a port number to the socket---*/
	if ( bind(sockfd, (struct sockaddr*)&self, sizeof(self)) != 0 )
	{
		perror("socket--bind");
		exit(errno);
	}
	else;

	if (UDP)
	{
		/*---Make it a "listening socket"---*/
		//check to see if this is the problem with recv stopping working
		if ( listen(sockfd, 20) != 0 )
		{
			perror("socket--listen");
			exit(errno);
		}
	}
	else;

	printf("Connected\n");

	//make connection loop



	if (!UDP)
	{
		/*---accept a connection (creating a data pipe)---*/
		clientfd = accept(sockfd, (struct sockaddr*)&client_addr, &addrlen);
		printf("%s:%d connected\n", inet_ntoa(client_addr.sin_addr), ntohs(client_addr.sin_port));
	}
	else;
}


InertialData::~InertialData() {
	if (!UDP)
		close(clientfd);
	else
		/*---Clean up (should never get here!)---*/
		close(sockfd);

	//CTextLcd::Release();
}

bool InertialData::isFalling() {
if (!UDP)
	ret = recv(clientfd, buffer, MAXBUF, 0);
else
	ret = recvfrom(sockfd,buffer,MAXBUF,0,(struct sockaddr *)&client_addr,&addrlen);

	//printf("recv ret: %d\n", ret);
	if(ret <= 0)
	{
		// I guess this means it's broken, so we'll have to figure out what to do
	}
	printf("Received\n");
	//printf("%s\n",buffer);

	if(0 == strncmp("falling",buffer,7))
	{
		printf("Falling\n");
		return true;
		// TODO ADD CODE FOR HAS FALLEN
	}
	else if(0 == strncmp("backward",buffer,8))
	{

	}
	else if(0 == strncmp("right",buffer,5))
	{

		//usleep(500000); //wait, and then store the new positions of the motors

	}
	else if(0 == strncmp("left",buffer,4))
	{

	}
	else if(0 == strncmp("stop",buffer,4))
	{

	}
	else if(0 == strncmp("speed",buffer,5))
	{
		//parse speed
		//speed = 1000*atoi(&buffer[5]);
		//printf("Speed = %d\n",speed);
	}
	else if(0 == strncmp("exit",buffer,4))
	{
		/*
				isBackwardCorrecting = false;
				isForwardCorrecting = false;
				break;
		 */
	}

	printf("%s\n",buffer);

	//zero out buffer
	memset(buffer, 0, MAXBUF);

	/*---Close data connection---*/
	//close(clientfd);

	//usleep(100000); //TODO: remove this

	return false;
}
